#!coding:utf-8
import time
import numpy as np
import os
import sys
import tty
import termios
from pupper.joystick_interface import JoystickInterface
from pupper.config import Configuration
from pi_driver import SServo


def readchar():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    # print(ch, ord(ch))
    return ord(ch)


def readkey(getchar_fn=None):
    getchar = getchar_fn or readchar
    c1 = getchar()
    if c1 == 0x03:
        exit()
    if c1 != 0x1b:
        return c1
    c2 = getchar()
    c3 = getchar()
    return c2*100+c3
    # sys.stdin.flush()


class KEY:
    ArrowLeft = 9168
    ArrowUp = 9165
    ArrowRight = 9167
    ArrowDown = 9166
    Enter = 13
    Memu = 7980  # KeyM (Menu)
    Back = 7981  # KeyB (Back)
    Run = 7982  # KeyR (Run)
    Stop = 27  # KeyS (Stop or Home)


'''
Enter 13
Up 9165
Down 9166 
Left 9168 
Right 9167
F1 7980
F2 7981
F3 7982
'''

calibrations = [0 for i in range(12)]
servo = SServo()


def save_config(calibrations):
    init_offset = np.array([servo.get_offset(i+1) for i in range(12)])
    new_offset = init_offset+np.array(calibrations)*2
    for i in range(12):
        servo.set_offset(i+1, new_offset[i])
    print('new offset',new_offset)

def main():
    global calibrations
    global servo
    joystick_interface = JoystickInterface(Configuration())
    msg = joystick_interface.get_joystick()
    n = 0
    step = 4
    while True:
        # os.system('clear')
        if n < 0:
            n = 0
        elif n >= 12:
            n = 11
        print("准备较准%d号舵机\n左右键修改中心位置\n上下键修改id\n,三角键保存,X键退出" % (n+1))
        print("当前校准值: %d" % calibrations[n])
        while True:
            last_msg = msg
            msg = joystick_interface.get_joystick()
            if abs(msg["lx"]+msg['dpady']+msg['dpadx']+msg['x']+msg['square']) > 0.5:
                break
            time.sleep(0.05)
        if last_msg['dpady'] != 1 and msg['dpady'] == 1:
            n = n-1
        elif last_msg['dpady'] != -1 and msg['dpady'] == -1:
            n = n+1
        elif last_msg['dpadx'] != -1 and msg['dpadx'] == -1:
            calibrations[n] = calibrations[n] - 1
        elif last_msg['dpadx'] != 1 and msg['dpadx'] == 1:
            calibrations[n] = calibrations[n] + 1
        elif msg["lx"] < -0.5:
            calibrations[n] = calibrations[n] - step
        elif msg["lx"] > 0.5:
            calibrations[n] = calibrations[n] + step
        elif last_msg['x'] == 0 and msg['x'] == 1:
            print(calibrations)
            break
        elif last_msg['square'] == 0 and msg['square'] == 1:
            print("正在保存，请稍等")
            save_config(calibrations)
            print("保存成功")
            time.sleep(1)
            break
        for i in range(12):
            servo.set_position(i+1, calibrations[i] + 1023/2)
        time.sleep(0.01)
        print(last_msg)


if __name__ == '__main__':
    main()
